import numpy as np
from cv2 import cv2
import re
import matplotlib.pyplot as plt
from PIL import Image
from torchvision import transforms
import torch
import torchvision.datasets as datasets
import random
from sklearn.cluster import KMeans
from sklearn import decomposition
import scipy.stats
import matplotlib.pyplot as plt
from scipy.stats import wasserstein_distance
from sklearn.metrics import confusion_matrix
import pickle

position_path1 = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round1/round1_sync_position.npy' # 经纬度
position_path2 = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/round2_sync_position.npy' # 经纬度
position_path3 = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round3/round3_sync_position.npy' # 经纬度
GNSS1 = np.load(position_path1)
GNSS2 = np.load(position_path2)
GNSS3 = np.load(position_path3)
GNSS = (GNSS1, GNSS2, GNSS3)

index_map = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/IndexMap.pickle'
index_map = open(index_map,'rb')
index_map = pickle.load(index_map)

key_round = 2
query_round = 1


no_match_cnt = 0
coarse_match_cnt = 0
mapping = index_map[key_round]
for i in range(np.shape(mapping)[0]):
    if mapping[i][query_round] == -1:
        no_match_cnt += 1
    else:
        x1 = GNSS[key_round][i][0]
        y1 = GNSS[key_round][i][1]
        yaw1 = GNSS[key_round][i][2]
        
        i = mapping[i][query_round]

        x2 = GNSS[query_round][i][0]
        y2 = GNSS[query_round][i][1]
        yaw2 = GNSS[query_round][i][2]

        dis = np.sqrt(np.power(x1-x2,2)+np.power(y1-y2,2))
        dyaw = abs(yaw1 - yaw2)
        if dyaw > np.pi:
            dyaw = 2*np.pi - dyaw
        dyaw = 180*dyaw / np.pi
        
        if dyaw > 30 or dis > 10:
            coarse_match_cnt += 1

print('total frame: ', np.shape(mapping)[0])
print('no match: ', no_match_cnt)
print('coarse match: ', coarse_match_cnt)
